/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot2012v2 extends SimpleRobot {

    // Declare a compressor variable
    Compressor compressor; // Air compressor

    // The following variables give names to the digital input channels
    final int SwitchChannelPressure = 4;

    //The following variables give names to the Relay Channels
    final int RelayChannelCompressor = 1;

    // declare variables to use for the different modes during teleoperated
    private static final int MODE_IDLE = 0;
    private static final int MODE_START_LIFTING = 1;
    private static final int MODE_LIFTING = 2;
    private static final int MODE_START_SHOOTING = 3;
    private static final int MODE_SHOOTING = 4;
    private static final int MODE_START_EJECTING = 5;
    private static final int MODE_EJECTING = 6;
    //declare variables for the max and min speed of the stager
    private static final double STAGER_MAX_SPEED = 1.0;
    private static final double STAGER_MIN_SPEED = 0.5;
    // define the max lifter time and stager time in seconds
    private static final double MAX_LIFTER_TIME = 5.0;
    private static final double MAX_STAGER_TIME = 5.0;
    // declare a variable to hold the current mode
    private static int mode = MODE_IDLE;
    // declare a timer to use for the lifter
    private static Timer lifterTimer = new Timer();
    // declare a timer to use for the stager
    private static Timer stagerTimer = new Timer();
    private static LimitSwitch inc = new LimitSwitch(DigitalSidecar.INPUT_INCREMENT_SWITCH);
    private static LimitSwitch dec = new LimitSwitch(DigitalSidecar.INPUT_DECREMENT_SWITCH);
    /*
     * This overrides the robotInit code in the SimpleRobot class.
     * It will be called exactly once when the robot is first turned on.
     */

    public void robotInit() {

        // create the air compressor
        compressor = new Compressor(SwitchChannelPressure, RelayChannelCompressor);
        compressor.start();

        Dashboard.setDriveStatus("OK");
        Dashboard.setLifterStatus("OK");
        Dashboard.setShooterStatus("OK");
        Dashboard.setStagerStatus("OK");
        Dashboard.setWedgeStatus("OK");
        Dashboard.setShooterSpeedStatus("0");

        // static constructors

        if (Drivetrain.init() == false) {
            System.out.println("Drivetrain.init failed");
        }
        if (DriverStick.init() == false) {
            System.out.println("DriverStick.init failed");
        }
        if (ShooterStick.init() == false) {
            System.out.println("ShooterStick.init failed");
        }
        if (Lifter.init() == false) {
            System.out.println("Lifter.init failed");
        }
        if (Stager.init() == false) {
            System.out.println("Stager.init failed");
        }
        if (Shooter.init() == false) {
            System.out.println("Shooter.init failed");
        }
        if (Wedge.init() == false) {
            System.out.println("Wedge.init failed");
        }
        BallCounter.reset();

        Dashboard.update();
    }

    // returns the maximum time in seconds of the power curve for autonomous mode based
    // on how the sliders are set. Returned value is between 0 and 1.
    public static double getMaxTime() {
        double MAX_DRIVE_TIME = 15.0;
        double sliderValue = DSControls.getSlider(DSControls.SLIDER_AUTONOMOUS_MAX_TIME);

        return sliderValue * MAX_DRIVE_TIME;
    }

    // returns the maximum speed of the power curve for autonomous mode based
    // on how the sliders are set. Returned value is between 0 and 1.
    public static double getMaxSpeed() {
        double sliderValue = DSControls.getSlider(DSControls.SLIDER_AUTONOMOUS_MAX_SPEED);

        return sliderValue;
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        try {
            if (!isEnabled()) {
                return;
            }

            BallCounter.reset();
            BallCounter.increment();
            BallCounter.increment();

            // get the maximum time for the power curve
            double maxTime = getMaxTime();

            // get the maximum speed for the power curve
            double maxSpeed = getMaxSpeed();

            // define an array that holds the relative speed values from time=0
            // at the beginning of the array and time=maxTime at the end of
            // the array. The array holds "relative speeds" because an entry of 0
            // means 0 speed, and 1 means maxSpeed
            // Arrays in Java are 0 based, so the first entry is at speedPercent[0]
            // and the last entry is at speedPercent[speedPercent.length - 1]
            double speedPercent[] = {0.0, 0.125, 0.4, 0.75, 1.0, 0.75, 0.4, 0.125, 0.0};

            // determine how much time for each interval of the power curve
            // maxTime is the total time
            // the number of intervals is speedPercent.length - 1
            // so the amount of time for each interval is:
            //   intervalTime = maxTime / numIntervals
            //                = maxTime / (speedPercent.length - 1)
            double intervalTime = maxTime / (speedPercent.length - 1);

            //disable watchdog
            getWatchdog().setEnabled(false);

            /*
             * deploy the wedge
             */
            Wedge.deploy();

            // create a timer to determine how long we have been running the
            // current interval in the power curve
            Timer timer = new Timer();
            timer.reset();
            timer.start();

            // create a variable to hold the current index in the power curve
            int currentIndex = 0;

            // obtain the initial speed percent from the array, and compute
            // the actual speed by multiplying the speed percent with maxSpeed
            double speed = speedPercent[currentIndex] * maxSpeed;

            // drive forward by the current speed
            Drivetrain.drive(speed, 0);

            // keep driving while we are enabled and in autonomous mode
            while (isAutonomous()) {
                // check to see if the timer has run longer than the intervalTime
                if (timer.get() > intervalTime) {
                    // increment the currentIndex
                    currentIndex = currentIndex + 1;

                    // if we are past the end of the power curve, break out
                    // of the loop
                    if (currentIndex >= speedPercent.length) {
                        break;
                    }

                    // obtain the current speed percent from the array, and compute
                    // the actual speed by multiplying the speed percent with maxSpeed
                    speed = speedPercent[currentIndex] * maxSpeed;

                    // drive forward by the current speed
                    Drivetrain.drive(-speed, 0);

                    // stop the timer, reset it, and start it again
                    timer.stop();
                    timer.reset();
                    timer.start();
                }

                // Sleep for 5 milliseconds to give the cRio a chance to
                // process other events
                Timer.delay(0.005);
            }

            // stop the robot
            Drivetrain.drive(0, 0);
        } catch (Exception ex) {
            Dashboard.setErrorStatus("Init Motor " + ex.getMessage());
            Dashboard.update();
            ex.printStackTrace();
        }
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        try {
            // enable the watchdog
            // the watchdog will automatically disable the robot if something
            // in the software hangs. As long as you feed the watchdog, it
            // will assume everything is ok. However, if you go too long
            // without feeding the watchdog, it will assume the software is
            // hung is disable the robot
            getWatchdog().setEnabled(true);

            // loop over the following instructions as long as the robot
            // is enabled and the mode is set to teleoperated (operator control)
            while (isEnabled() && isOperatorControl()) {
                // always feed the watchdog first to let it know everything is ok
                getWatchdog().feed();

                BallCounter.teleopUpdate();

                // update the Drivetrain
                Drivetrain.teleopUpdate();

                // update the Wedge
                Wedge.teleopUpdate();

                // update the Shooter
                Shooter.teleopUpdate();

                // update the rest of the robot based on a state machine
                runStateMachine();

                // update the Dashboard
                Dashboard.teleopUpdate();

                // Sleep for 5 milliseconds to give the cRio a chance to
                // Process other events
                Timer.delay(.005);
            }
        } catch (Exception ex) {
            Dashboard.setErrorStatus("Init Motor " + ex.getMessage());
            Dashboard.update();
            ex.printStackTrace();
        }
    }

    /*
     * runStateMachine is called repeatedly during teleoperated mode to
     * update several components of the robot based on a state machine
     */
    private static void runStateMachine() {
        Dashboard.setModeStatus("" + mode);

        boolean shooterEnabled = !ShooterStick.disableShooter();
        boolean lifterDisabled = !DSControls.getButton(DSControls.BUTTON_LIFTER_ENABLE);
        boolean lifterEnabled = !lifterDisabled;
        boolean stagerDisabled = !DSControls.getButton(DSControls.BUTTON_STAGER_ENABLE);
        boolean stagerEnabled = !stagerDisabled;

        switch (mode) {
            case MODE_IDLE:
                // check to see if we should run the lifter or stop it
                boolean manualLifterOverride = ShooterStick.manualLifterPressed()
                        && lifterEnabled;
                boolean manualStagerOverride = ShooterStick.manualStagerPressed()
                        && stagerEnabled;
                boolean tooManyBalls = BallCounter.get() >= 3;

                if (manualLifterOverride) {
                    //run the lifter if the manual button is pressed
                    //AND if the lifter is enabled.
                    Lifter.start();
                } else if (tooManyBalls
                        || lifterDisabled) {
                    // stop the lifter
                    Lifter.stop();
                } else {
                    // if none of the above, run the lifter
                    Lifter.start();

                    // check to see if we should lift a ball
                    // Note this code is only called when the
                    // lifter is running
                    if (inc.isRisingEdge()) {
                        mode = MODE_START_LIFTING;
                        break;
                    }
                }
                if (manualStagerOverride) {
                    Stager.start(STAGER_MIN_SPEED);
                }

                // check to see if we should start shooting
                if (shooterEnabled && (ShooterStick.shootRisingEdge()
                        || (ShooterStick.shootPressed() && ShooterStick.fullAutoPressed()))) {
                    mode = MODE_START_SHOOTING;
                    break;
                }

                // check to see if we should start ejecting
                if (stagerEnabled && ShooterStick.reverseRisingEdge()
                        || (ShooterStick.reversePressed() && ShooterStick.fullAutoPressed())) {
                    mode = MODE_START_EJECTING;
                    break;
                }
                break;
            case MODE_START_LIFTING:
                if (lifterEnabled) {
                    startLifting();
                }
                break;
            case MODE_LIFTING:
                if (lifterEnabled) {
                    updateLifting();
                }
                break;
            case MODE_START_SHOOTING:
                if (shooterEnabled) {
                    startShooting();
                }
                break;
            case MODE_SHOOTING:
                if (shooterEnabled) {
                    updateShooting();
                }
                break;
            case MODE_START_EJECTING:
                if (stagerEnabled) {
                    startEjecting();
                }
                break;
            case MODE_EJECTING:
                if (stagerEnabled) {
                    updateEjecting();
                }
                break;
        } // end of switch (any break above jumps to here)
    } // end of "while enabled"

    /*
     * startLifting is called to initiate the process of lifting a ball
     * into the robot
     */
    private static void startLifting() {
        // start the lifter
        lifterTimer.reset();
        lifterTimer.start();
        Lifter.start();

        // start the stager
        stagerTimer.reset();
        stagerTimer.start();
        Stager.start(STAGER_MIN_SPEED);

        // change mode to LIFTING
        mode = MODE_LIFTING;
    }


    /*
     * updateLifting is called to repeatedly while the robot is lifting
     * a ball to determine when to stop
     */
    private static void updateLifting() {
        // define the lifter time in seconds
        // it is computed as the percentage of the max lifter time according
        // to a slider
        double lifterPercent = DSControls.getSlider(DSControls.SLIDER_LIFTER_TIMER_PERCENT);
        double lifterDuration = lifterPercent * MAX_LIFTER_TIME;

        double stagerPercent = DSControls.getSlider(DSControls.SLIDER_STAGER_TIMER_PERCENT);
        double stagerDuration = stagerPercent * MAX_STAGER_TIME;

        double lifterTime = lifterTimer.get();
        double stagerTime = stagerTimer.get();

        /*System.out.println("lifter(MAX,%,DUR,T)="
        + MAX_LIFTER_TIME + " " + lifterPercent + " " + lifterDuration + " " + lifterTime + " "
        + "stager(MAX,%,DUR,T)="
        + MAX_STAGER_TIME + " " + stagerPercent + " " + stagerDuration + " " + stagerTime);*/

        // if the lifter has been running more than the lifter duration, stop the lifter
        if (Lifter.isRunning() && (lifterTime > lifterDuration)) {
            Lifter.stop();
            lifterTimer.stop();
        }

        // if the stager has been running more than the stager duration, stop the stager
        if (Stager.isRunning() && (stagerTime > stagerDuration)) {
            Stager.stop();
            stagerTimer.stop();
        }

        // if both timers have expired, then increment the ball count and set the mode
        // back to idle
        if (Lifter.isRunning() == false && Stager.isRunning() == false) {
            BallCounter.increment();

            //If the stager timer goes past stagerDuration then the stager will stop
            //and the stager timer will stop.
            mode = MODE_IDLE;
        }
    }

    private static void startShooting() {
        // no work to do here other than changing mode
        mode = MODE_SHOOTING;
    }

    private static void updateShooting() {
        // NOTE: Previously the code to stop the lifter and start the stager
        // was in the startShooting method, but we were seeing instances where
        // the stager would stop running. So we keep updating it in here
        // to make sure it never stops running. Even turning off motor
        // safety enable did not fix it.

        // stop the lifter
        // NOTE: Because we don't have a limit switch currently, we are going to
        // let the lifter keep running when we are shooting. That way we can
        // turn on the stager to lift a ball by shooting.
        // Lifter.stop();

        // start the stager
        // reduce stager's speed to half
        Stager.start(STAGER_MAX_SPEED);

        boolean shootingComplete = false;

        boolean shootPressed = ShooterStick.shootPressed();
        boolean ballShot = dec.isFallingEdge();
        boolean fullAuto = ShooterStick.fullAutoPressed();

        //System.out.println("trigger=" + shootPressed + " shot=" + ballShot + " auto=" + fullAuto);

        // check to see if the shoot button is still pressed
        if (shootPressed) {
            // if the decrement counter sees a falling edge, it means
            // a ball has been shot. In that case, decrement the number
            // of balls and stop shooting if we are not in full auto mode
            if (ballShot) {
                BallCounter.decrement();
                if (fullAuto == false) {
                    shootingComplete = true;
                }
            }
        } else {
            // set shooting complete to true because the shoot button
            // is no longer pressed
            shootingComplete = true;
        }

        // if shooting is complete, stop the stager and return
        // to idle mode
        if (shootingComplete) {
            Stager.stop();
            mode = MODE_IDLE;
        }
    }

    private static void startEjecting() {
        // no work to do here other than changing mode
        mode = MODE_EJECTING;
    }

    private static void updateEjecting() {
        // NOTE: Previously the code to reverse the lifter and stager
        // was in the startShooting method, but we were seeing instances where
        // the stager would stop running. So we keep updating it in here
        // to make sure it never stops running. Even turning off motor
        // safety enable did not fix it.

        // run the lifter and the stager in reverse
        Lifter.reverse();
        Stager.reverse();
        Shooter.reverse();

        boolean ejectingComplete = false;

        // check to see if the eject button is still pressed
        if (ShooterStick.reversePressed()) {
            // if the increment counter sees a falling edge, it means
            // a ball has been ejected. In that case, decrement the number
            // of balls
            if (inc.isFallingEdge()) {
                BallCounter.decrement();
            }
        } else {
            // set ejecting complete to true because the reverse button
            // is no longer pressed
            ejectingComplete = true;
        }

        // if ejecting is complete, stop the lifter and stager and return
        // to idle mode
        if (ejectingComplete) {
            Lifter.stop();
            Stager.stop();
            mode = MODE_IDLE;
        }
    }
}
